//////////////////////////////
// serial buffer loading
const int _iMaxSerialBuffer = 4092;
int _iSerialBufferStart=0, _iSerialBufferLen=0, _iSerialBufferIndex=0;
char _SerialBuffer[_iMaxSerialBuffer];
boolean readSerialString (String &cmd) {
  int sb;
  while (Serial1.available())
  {
    sb = Serial1.read();
    if(sb=='#') // start cmd ?
    {
      _iSerialBufferStart = _iSerialBufferIndex+1;
    }
    else if(sb=='$' && _iSerialBufferStart>=0) // end cmd ?
    {
      _iSerialBufferLen = _iSerialBufferIndex;
      _SerialBuffer[_iSerialBufferLen] = '\0';
      cmd = &_SerialBuffer[_iSerialBufferStart];
      _iSerialBufferIndex = 0;
      _iSerialBufferStart = -1;
      return true;
    }
    if(_iSerialBufferIndex>=_iMaxSerialBuffer)
    {
      _iSerialBufferIndex = 0;
      _iSerialBufferStart = -1;
      return false;
    }
    _SerialBuffer[_iSerialBufferIndex] = sb;
    _iSerialBufferIndex++;
  }

  return false;
}

///////////////////////////////
// check commands sended
int load_int(String &s, const int start, int &iRet)
{
  int iLen = start+1;
  String num="";

  if (s[start] == '-') num += "-";
  else if(s[start] == '+') num += "";
  else return start;

  for(int i=1; i<6 && s.length()>(start+i); i++)
  {
    char c = s[start+i];
    if(!isDigit(c))
      break;
    num += c;
    iLen ++;
  }

  iRet = num.toInt();
  return iLen;
}
int load_int_nosigne(String &s, const int start, int &iRet)
{
  int iLen = start;
  String num="";

  if (s[start] == '-') num += "-";
  else if(s[start] == '+') num += "";

  for(int i=0; i<6 && s.length()>(start+i); i++)
  {
    char c = s[start+i];
    if(!isDigit(c))
      break;
    num += c;
    iLen ++;
  }

  iRet = num.toInt();
  return iLen;
}

boolean check_commands(void)
{
  String line="";
  boolean bReturn = false;
  while(readSerialString(line))
  {
    if(line.length()==0) continue;
    
    if(g_isDebug) Serial.println(line);
    // debug only : send_message(line);

    switch(line[0])
    {
    case 'c': // (c)onnect
      {
        // #c$
        if(g_isDebug) Serial.println("Receive BT connect");
        send_ready(g_isReady);
      }
      break;
    case 'v': // (V)ersion
      {
        // #v$
        send_version();
      }
      break;
    case 's': // (S)ervo
      {
        // #s+0+90$
        int servo=0;
        int angle=0;
        int next = 1;
        next = load_int(line, next, servo);
        next = load_int(line, next, angle);
        set_servo_position(servo, angle);
      }
      break;
    case 'p': // (P)ositions
      {
        //Serial.println(line);
        
        // #p+1+0+90+1+92$
        int count=0;
        int servo=0;
        int angle=0;
        int max_dep=0, dep=0;
        int next = 1;
        next = load_int(line, next, count);
        for(int c=0; c<count; c++)
        {
          next = load_int(line, next, servo);
          next = load_int(line, next, angle);
          dep = set_servo_position(servo, angle);
          if(dep>max_dep)
            max_dep = dep;
        }
        
        //if(max_dep>0) delay(max_dep*1.5); // wait end of servo placement ;)
      }
      break;
    default:
      {
        String sError = "invalid cmd '" + line + "'";
        send_error(sError);
        Serial.println(sError);
      }
      break;
    }
    bReturn = true;
  }
  return bReturn;
}



